import subprocess
import time
import os

def main():
    # 切换到 catkin_ws 目录
    #os.chdir(os.path.expanduser("~/catkin_ws"))
    
    # 加载 ROS 环境变量
    subprocess.run("source ~/.bashrc", shell=True, executable='/bin/bash')
    print("Source environment")
    
    # 修改串口权限
    subprocess.run("sudo chmod 777 /dev/ttyUSB0", shell=True)
    
    # 启动 MAVROS
    mavros_process = subprocess.Popen(
        ["gnome-terminal", "--", "bash", "-c", "roslaunch mavros apm.launch; exec bash"]
    )
    time.sleep(5)  # 确保 MAVROS 启动完成
    
    # 设置 MAVROS 速率
    #command=["rosrun", "mavros", "mavsys", "rate", "--all", "10"]
    
    rate_process = subprocess.Popen(["gnome-terminal", "--", "bash", "-c", "sleep 2; rosrun mavros mavsys rate --all 10; exec bash"])
    print("rate_change")
    
    # 启动 AR 视觉跟踪
    ar_tracking_process = subprocess.Popen(
        ["gnome-terminal", "--", "bash", "-c", "sleep 2; roslaunch yahboomcar_visual ar_track_csi.launch; exec bash"]
    )
    time.sleep(4)  # 确保 AR 追踪启动
    
    # 启动 GPS + 视觉降落
    gps_landing_process = subprocess.Popen(
        ["gnome-terminal", "--", "bash", "-c", "sleep 6; rosrun iq_gnc 124GPSvisionland; exec bash"]
    )
    
    print("All ROS nodes have been launched successfully.")

if __name__ == "__main__":
    main()